local state = {}
state.name = "AIMING INDIRECT"
state.id = 3

if(SERVER) then

  include("includes/enum/disposition.lua")
  
  state.startTime = nil
  state.targetPos = nil
  state.targetEnt = nil
  state.stagingPoint = nil
  state.maxAimingTime = 60
  state.targetingResolution = 50
  state.groupRadius = 200

  function state.New(target)
    local newState = table.Copy(state)
    newState.target = target
    
    return newState
  end

  function state:Run(ag3)
    if(ag3:IsGrouped() == false) then
      return ag3:SetNextState(ag3.States.Idle)
    end
  
    if(self.startTime == nil) then
      self.startTime = CurTime()
    end
    
    local isAimingTimeElapsed = (CurTime() - self.startTime) > self.maxAimingTime
    
    if(isAimingTimeElapsed || ag3:IsArmed() ~= true) then
      return ag3:SetNextState(ag3.States.Idle)
    end
    
    local group = ag3:GetGroup()
  
    if(ValidEntity(self.targetEnt)) then
      self.targetPos = self.targetEnt:GetPos()
    else
      self.targetPos = group.target
      
      if(self.targetPos ~= nil) then
        local targetEntDistanceFromPoint = nil
        
        for _, entity in pairs(ents.FindInSphere(self.targetPos, self.targetingResolution)) do
          local distanceFromPoint = entity:GetPos():Distance(self.targetPos)
        
          if(ValidEntity(entity) && entity:IsWorld() == false) then
            if(ValidEntity(self.targetEnt) == false || distanceFromPoint < targetEntDistanceFromPoint) then
              self.targetEnt = entity
              targetEntDistanceFromPoint = distanceFromPoint
            end
          end
        end
      else
        return ag3:SetNextState(ag3.States.Idle)
      end
      
      if(ValidEntity(self.targetEnt) && self.targetEnt:IsNPC()) then
        self.targetEnt:AddEntityRelationship(ag3, D_FR, 99)
      end
    end
    
    if(group.focalPoint == nil) then
      group.focalPoint = self:DetermineFocalPointForGroup(group)
    end
    
    if(self.stagingPoint == nil) then
      self.stagingPoint = self:DetermineStagingPoint(ag3)
    end
    
    local targetAngle = (group.focalPoint - ag3:GetPos()):Angle()
    
    self:UseAimingPhysics(ag3, self.stagingPoint, targetAngle)
    
    local isInPosition = ag3:GetPos():Distance(self.stagingPoint) < 1
    local isAimedAtTarget = ag3:GetForward():Distance(targetAngle:Forward()) < 0.01
    
    if(isInPosition) then
      DebugMsg(tostring(ag3) .. " is in position.\n")
    else
      DebugMsg(tostring(ag3) .. " is " .. ag3:GetPos():Distance(self.stagingPoint) .. "ft from staging point.\n")
    end
    
    if(isAimedAtTarget) then
      DebugMsg(tostring(ag3) .. " is aimed at target.\n")
    else
      DebugMsg(tostring(ag3) .. " is aiming at target.\n")
    end
    
    if(isInPosition && isAimedAtTarget && self:HasLOS(ag3)) then
      return ag3:SetNextState(ag3.States.Charging)
    end
    
    return self
  end
  
  function state:HasLOS(ag3)
    local group = ag3:GetGroup()
  
    local traceSpec = {}
    traceSpec.start = ag3:GetPos()
    traceSpec.endpos = group.focalPoint
    traceSpec.filter = ag3
    local traceToFocalPoint = util.TraceLine(traceSpec)
  
    if(traceToFocalPoint.Fraction >= 0.99) then
      DebugMsg(tostring(ag3) .. " has LOS to focal point.\n")
    
      traceSpec = {}
      traceSpec.start = group.focalPoint
      traceSpec.endpos = self.targetPos
      local traceToTarget = util.TraceLine(traceSpec)
      
      if(traceToTarget.Fraction >= 0.99) then
        DebugMsg(tostring(ag3) .. " has LOS to target.\n")
        return true
      elseif(ValidEntity(self.targetEnt) && ValidEntity(traceToTarget.Entity) && traceToTarget.Entity == self.targetEnt) then
        DebugMsg(tostring(ag3) .. " has LOS to " .. tostring(self.targetEnt) .. ".\n")
        return true
      else
        DebugMsg(table.ToString(traceToTarget, "Trace to target for " .. tostring(ag3), true))
        DebugMsg(tostring(ag3) .. " does not have LOS to target.\n")
        return false
      end
    else
      DebugMsg(table.ToString(traceToFocalPoint, "Trace to focal point for " .. tostring(ag3), true))
      DebugMsg(tostring(ag3) .. " does not have LOS to focal point.\n")
      return false
    end
  end
  
  function state:UseAimingPhysics(ag3, stagingPoint, targetAngle)
    ag3.PhysicsSimulate = function(entity, physics, timeDelta)
      physics:Wake()
      
      local shadowParams = {}
      shadowParams.secondstoarrive = 1
      shadowParams.pos = stagingPoint
      shadowParams.angle = targetAngle
      shadowParams.maxangular = 500
      shadowParams.maxangulardamp = 2000
      shadowParams.maxspeed = 50
      shadowParams.maxspeeddamp = 200
      shadowParams.dampfactor = 0.6
      shadowParams.teleportdistance = 0
      shadowParams.deltatime = timeDelta
     
      physics:ComputeShadowControl(shadowParams)
    end
    
    ag3:StartMotionController()
    
    local ag3Physics = ag3:GetPhysicsObject()
    
    if(ag3Physics && ag3Physics:IsValid()) then
      ag3Physics:Wake()
    end
  end
  
  function state:DetermineStagingPoint(ag3)
    local group = ag3:GetGroup()
    local index = nil
    
    for i, groupMember in ipairs(group) do
      if(groupMember == ag3) then
        index = i
        break
      end
    end
    
    if(index == nil) then
      error(tostring(ag3) .. " is not in its group!")
    end
    
    if(group.focalPoint == nil) then
      group.focalPoint = self:DetermineFocalPointForGroup(group)
    end
    
    local focalPointVector = group.focalPoint - self.targetPos
    local stagingPoint = group.focalPoint + (focalPointVector:GetNormal() * (self.groupRadius * 1.5))

    local uniqueRotation = (index / #group * 360) + (1 / #group * 360 / 2)
    local rotMatrix = MMatrix.RotationMatrix(focalPointVector:GetNormal(), uniqueRotation)
    local uniqueDisplacement = (rotMatrix * focalPointVector:Angle():Up()) * self.groupRadius
    
    return stagingPoint + uniqueDisplacement
  end
  
  function state:DetermineFocalPointForGroup(group)
    local groupCentre = Vector(0, 0, 0)
    local groupMinRange = 0
    
    for _, ag3 in ipairs(group) do
      local minRange = ag3:GetMinimumRange()
      
      if(minRange > groupMinRange) then
        groupMinRange = minRange
      end
      
      groupCentre = groupCentre + ag3:GetPos()
    end
    
    groupCentre = groupCentre / #group
    
    local targetVector = (self.targetPos - groupCentre):GetNormal()
    local focalPoint = groupCentre + (targetVector * (self.groupRadius * 1.5))
    
    local focalPointVector = focalPoint - self.targetPos
    local distanceToTarget = focalPointVector:Length()
    
    if(distanceToTarget < groupMinRange) then
      focalPoint = focalPoint + (focalPointVector:GetNormal() * (groupMinRange - distanceToTarget))
    end
    
    return focalPoint
  end
  
end

ENT.States.AimingIndirect = state
